#!/usr/bin/python
# 6 circle
# -*- coding: UTF-8 -*-
from turtle import position
import rospy
import PyKDL
import tf
from mavros_msgs.msg import State, PositionTarget 
from mavros_msgs.srv import CommandBool, SetMode, SetMavFrame
from geometry_msgs.msg import Twist, Point, PoseStamped, TwistStamped,Pose
from std_msgs.msg import String
import numpy
import math
import sys
import copy
#sys.path.append('/home/malan/catkin_robocup/devel/lib/python2.7/dist-packages')
from catch_actors.msg import coordinate, auction, auction_value, tracking
from ObstacleAvoid import ObstacleAviod

class CatchingPlan:

    def __init__(self, uav_id):
        # variables of uavs' controller
        self.uav_type = 'typhoon_h480'
        self.id = int(uav_id)
        self.uav_num = 6
        self.bais = numpy.array([[0.0, 3.0], [3.0, 3.0], [0.0, 6.0], [3.0, 6.0], [0.0, 9.0], [3.0, 9.0]])  # chu shi wei zhi pian zhi (tong yi wei gazebo zuo biao)
        #self.bais = numpy.array([[0.0, 0.0], [3.0, 3.0], [6.0, 3.0], [0.0, 0.0], [3.0, 0.0], [6.0, 0.0]])
        # self.black_box = [[[-35.375, -34.825], [-4, -3]], [[-3.275, -2.725], [-4, -3]], [[20.725, 21.275], [-4, -3]], \
        # [[57.725, 58.275], [-4, -3]], [[83.725, 84.275], [-4, -3]], [[-25.475, -24.925], [3, 4]], [[8.725, 9.275], [3, 4]], \
        # [[26.725, 27.275], [3, 4]], [[70.725, 71.275], [3, 4]], [[90.225, 90.775], [3, 4]],[[53.2, 54.2],[-9.0, -6.72]], \
        # [[79, 95], [-34, -27]],[[3, 9], [34, 36]], [[-35.0, -19.0], [17.0, 36]], [[4.0, 22.0], [11.0, 25.0]], [[55, 69.0], [15, 30.3]], \
        # [[71.5, 83.5], [9.2, 18.5]], [[88.3, 100.7], [11.9, 16.2]], [[79, 94.7], [22.2, 34.5]], [[53, 70], [-35, -27]], [[-5.4, 7.0], [-19.4, -10.0]], \
        # [[18.5, 39.5], [-22.5, -5.5]], [[-5.0, 5.0], [-34.0, -22.0]], [[-27.8, -23.5], [-27.5, -15]], [[-36, -24], [-35, -31]], \   
        #[[-35.5, -31.3], [-25.5, -13]], [[3.0,8.2],[34.3,35.7]]]  # zhang ai wu
        self.black_box = [\
        [[-35.25, -18.51], [16.40, 35.38]],\
        [[2.80, 9.50], [33.50, 36.47]],\
        [[3.80, 23.45], [10.37, 25.62]],\
        [[52.54, 70.54], [14.43, 30.38]],\
        [[71.08, 83.62], [8.38, 18.61]],\
        [[78.63, 94.79], [21.42, 34.51]],\
        [[87.61, 100.91], [11.47, 16.40]],\
        [[-36.45, -34.63], [-4.32, -2.79]],\
        [[-26.59, -24.49], [2.69, 4.23]],\
        [[-4.48, -2.41], [-4.19, -2.85]],\
        [[7.57, 9.60], [2.82, 4.13]],\
        [[19.51, 21.58], [-4.19, -2.84]],\
        [[25.54, 27.60], [2.76, 4.21]],\
        [[51.92, 55.27], [-10.24, -4.81]],\
        [[-36.07, -30.59], [-26.47, -12.54]],\
        [[-28.08, -23.38], [-27.77, -14.45]],\
        [[-36.14, -23.60], [-35.45, -30.39]],\
        [[-6.13, 8.60], [-34.60, -10.48]],\
        [[17.74, 39.49], [-22.51, -5.50]],\
        [[51.2, 70.60], [-37.8, -23.50]],\
	    [[57.10, 66.48],[-24.77,-9.63]],\
        [[78.62, 92.60], [-34.24, -27,73]],\
        [[56.51, 58.57], [-4.19, -2.81]],\
        [[69.65, 71.57], [2.76, 4.20]],\
        [[82.51, 84.64], [-4.21, -2.79]],\
        [[88.72, 91.06], [2.78, 4.20]]]
        self.box_num = len(self.black_box)
        self.f = 30  # pin lv
        self.count = 0
        self.local_pose = PoseStamped()
        self.following_local_pose = [PoseStamped() for i in range(self.uav_num)]
        self.following_local_pose_sub = [None]*(self.uav_num)
        self.auction_sub = [None]*(self.uav_num)
        self.uav_current_pose = Point()
        self.uav_current_yaw = 0.0
        self.uav_vel = Twist()
        self.arrive_point = False
        self.Kp = 0.5
        self.Kpy = 1
        self.Kpvel = 1
        self.z = 4.5  # height
        self.velxy_max = 4
        self.velz_max = 3
        self.angz_max = 3
        self.target_position = Twist()
        self.target_yaw = 0.0
        self.last_yaw = 0.0
        self.tracking_id = tracking()
        self.arrive_count = 0
        self.cmd = ''
        #self.actor_others = 0
        self.file = open(str(uav_id)+'uav_vel.txt','w')

        # obstacle avoidance:
        self.safe_dis = 10.0  # distance between uavs for avoidance
        self.safe_height = 0.5
        self.Obstacleavoid = ObstacleAviod()  #ji wu 
        self.avoid_finish_flag = True
        self.subtarget_count = 0
        self.subtarget_length = 0
        self.avoid_start_flag = False
        self.subtarget_pos = []
        self.mid_target_position = Point()

        # variables of trajectory
        self.circle0_flag = False
        self.circle1_flag = False
        self.circle2_flag = False
        self.circle3_flag = False
        self.circle4_flag = False
        self.circle5_flag = False
        # self.circle2_flag = False
        #self.outer_flag = False
        self.init_situation_flag = True
        self.next_node = 0
        #self.situation2_flag = coordinate()  # check if it is one of the outer ones
        #self.situation2_list = [0 for i in range(self.uav_num)]  # list of uavs on the outer trajectory
        #self.circle1_pose = numpy.array([[-29.0, -29], [-29.0, 6.0], [13.0, 6.0], [13.0, -20.0], [-12.0, -20.0], [-12.0, -29]])
        self.circle4_pose = numpy.array([[-45.0,10.0], [-45.0,40.0], [40.0,40.0],[40.0,0.0],[-5.0,0.0]])
        self.circle2_pose = numpy.array([[-20.0,-10.0],[-15.0,-40.0],[-40.0,-40.0],[-40.0,45.0],[-10.0,45.0],[-10.0,0.0]])
        self.circle5_pose = numpy.array([[0.0,40.0],[120.0,40.0],[120.0,0.0],[0.0,0.0]])
        self.circle0_pose = numpy.array([[45.0,0.0],[45.0,-40.0],[0.0,-40.0],[-40.0,-38.0],[-40,0.0],[10,0.0]])
        self.circle3_pose = numpy.array([[110.0,0.0],[110.0,40.0],[50.0,40.0],[50.0,-45.0],[110.0,-45.0]])
        self.circle1_pose = numpy.array([[70.0,0.0],[70.0,-20],[110.0,-30.0],[110.0,-45.0],[17.0,-45.0],[17.0,0.0]])
        #self.circle1_center = auction()
        #self.circle1_center.auction_class = 'circle'
        #self.circle1_center.actor_x = 45.0
        #self.circle1_center.actor_y = 0.0
        self.circle0_center = auction()
        self.circle0_center.auction_class = 'circle'
        self.circle0_center.actor_x = 30.0
        self.circle0_center.actor_y = -20.0
        #
        self.circle1_center = auction()
        self.circle1_center.auction_class = 'circle'
        self.circle1_center.actor_x = 60.0
        self.circle1_center.actor_y = -20.0
        #
        self.circle2_center = auction()
        self.circle2_center.auction_class = 'circle'
        self.circle2_center.actor_x = -30.0
        self.circle2_center.actor_y = -20.0
        #
        self.circle3_center = auction()
        self.circle3_center.auction_class = 'circle'
        self.circle3_center.actor_x = 80.0
        self.circle3_center.actor_y = 20.0
        #
        self.circle4_center = auction()
        self.circle4_center.auction_class = 'circle'
        self.circle4_center.actor_x = -30.0
        self.circle4_center.actor_y = 20.0
        #
        self.circle5_center = auction()
        self.circle5_center.auction_class = 'circle'
        self.circle5_center.actor_x = 20.0
        self.circle5_center.actor_y = 20.0
        # situation flag: 0: height determination / 1: initial position / 2: general trajectory / 3: target tracking /
        #                 4: taskover position / 5: on the way to auction / 6: search for the actor (auction)
        self.situation_flag = 0
        self.search_flag = False

        # variables of auction algorithm
        self.auction_value = auction_value()
        self.auction_pos = auction()
        self.track_time = 0
        # self.reset_auction_flag = False
        self.task_num = 0  # jiao yan
        self.auction_finish_flag = True
        self.task_flag = False
        self.task_count = 0   # check if get all the auction prices
        self.task_position = auction()
        self.free_uav_flag = False # check if there is a uav join in the auction
        self.change_task_flag = False
        self.auction_value_all = []
        # self.waitingforauction_flag = False
        self.auction_count = 0
        self.fly_success_num = []  # the number of uavs the fly successfully
        self.takeoff_position = Pose()
        self.mavros_state = 'MANUAL'
        #variables of rostopic
        rospy.init_node('uav'+str(self.id))
        self.local_pose_sub = rospy.Subscriber(self.uav_type + '_' + str(self.id) + "/mavros/local_position/pose",
                                               PoseStamped, self.local_pose_callback)
        self.vel_enu_pub = rospy.Publisher('/xtdrone/'+self.uav_type+'_'+str(self.id)+'/cmd_vel_flu', Twist, queue_size=10)
        # self.situation2_id_pub = rospy.Publisher('situation2_id', coordinate, queue_size=10)   # topic for checking who are on the outer trajectory
        # self.situation2_id_sub = rospy.Subscriber('situation2_id', coordinate, self.situation2_callback)
        self.auction_pub = rospy.Publisher(self.uav_type + '_' + str(self.id) + '/auction', auction, queue_size=10)
        #self.tracking_pub = rospy.Publisher(self.uav_type + '_' + str(self.id) + '/tracking', tracking, queue_size=10)
        self.tracking_sub = rospy.Subscriber(self.uav_type + '_' + str(self.id) + '/start_tracking', auction_value, self.tracking_callback) # jump into tracking code or coming back
        self.auction_value_pub = rospy.Publisher('auction_value', auction_value, queue_size=10)
        self.auction_value_sub = rospy.Subscriber('auction_value', auction_value, self.auction_value_callback)
        self.cmd_pub = rospy.Publisher('/xtdrone/'+self.uav_type+'_'+str(self.id)+'/cmd',String,queue_size=10)
        self.start_yolo_pub = rospy.Publisher(self.uav_type+'_'+str(self.id)+'/start_yolo',String,queue_size=10) # uav fen san kai hou zai kai qi yolo
        self.takeoff_pub = rospy.Publisher(self.uav_type+'_'+str(self.id)+"/mavros/setpoint_raw/local", PositionTarget, queue_size=30)
        self.mavros_sub = rospy.Subscriber(self.uav_type+'_'+ str(self.id)+"/mavros/state", State, self.mavros_state_callback)
        self.arm_state = False
        self.armService = rospy.ServiceProxy(self.uav_type+'_'+str(self.id)+"/mavros/cmd/arming", CommandBool)
        self.flightModeService = rospy.ServiceProxy(self.uav_type+'_'+str(self.id)+"/mavros/set_mode", SetMode)
    def construct_target(self):
        target_raw_pose = PositionTarget()
        target_raw_pose.coordinate_frame = 1  
        target_raw_pose.position.x = self.takeoff_position.position.x - self.bais[self.id][0]
        target_raw_pose.position.y = self.takeoff_position.position.y - self.bais[self.id][1]
        target_raw_pose.position.z = self.takeoff_position.position.z
        target_raw_pose.yaw = self.target_yaw
        target_raw_pose.type_mask = PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \
                        + PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \
                        + PositionTarget.IGNORE_YAW_RATE
        return target_raw_pose
    
    def vel_cmd(self):
        target_raw_pose = PositionTarget()
        target_raw_pose.coordinate_frame = 8
        target_raw_pose.velocity.x = self.uav_vel.linear.x
        target_raw_pose.velocity.y = self.uav_vel.linear.y
        target_raw_pose.velocity.z = self.uav_vel.linear.z    
        target_raw_pose.yaw_rate = self.uav_vel.angular.z 
        target_raw_pose.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ \
                            + PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \
                            + PositionTarget.IGNORE_YAW
        return target_raw_pose
    
    def mavros_state_callback(self, msg):
        self.mavros_state = msg.mode
        self.arm_state = msg.armed
    def arm(self):
        if self.armService(True):
            return True
    def flight_mode_set(self, mode):
        """ mode selectable
        MANUAL, ACRO, ALTCTL, POSCTL, OFFBOARD, STABILIZED, RATTITUDE
        AUTO.MISSION, AUTO.LOITER, AUTO.RTL, AUTO.LAND, AUTO.RTGS, AUTO.READY, AUTO.TAKEOFF
        """
        if self.flightModeService(custom_mode=mode):
            return True
        else:
            self.flightModeService('OFFBOARD')
            print (str(self.id)+":Main loop"+mode + "Failed")
            self.file.write ("Main loop"+mode + "Failed")
            self.file.write('\n')


    def local_pose_callback(self, msg):
        self.local_pose = msg
        self.uav_current_pose = self.local_pose.pose.position
        self.uav_current_pose.x = self.uav_current_pose.x + self.bais[self.id][0]
        self.uav_current_pose.y = self.uav_current_pose.y + self.bais[self.id][1]
        self.uav_current_pose.z = self.uav_current_pose.z
        # change Quaternion to TF:
        x = self.local_pose.pose.orientation.x
        y = self.local_pose.pose.orientation.y
        z = self.local_pose.pose.orientation.z
        w = self.local_pose.pose.orientation.w
        rot = PyKDL.Rotation.Quaternion(x, y, z, w)
        
        res = rot.GetRPY()[2]
        while res > math.pi:
            res -= 2.0*math.pi
        while res < -math.pi:
            res += 2.0*math.pi

        if res < 0:
            res = res + 2.0 * math.pi
        self.uav_current_yaw = res  # 0 to 2pi
        
        #a = rot.GetRPY()
        #print(a)
        #br = tf.TransformBroadcaster()
        #br.sendTransform((self.uav_current_pose.x, self.uav_current_pose.y, 0), (x, y, z, w), rospy.Time.now(),
        #                 '/xtdrone/'+self.uav_type+'_'+str(self.id)+'_uavmap', "world")

    def following_local_pose_callback(self, msg, id):
        self.following_local_pose[id] = msg
        self.following_local_pose[id].pose.position.x = self.following_local_pose[id].pose.position.x + self.bais[id][0]
        self.following_local_pose[id].pose.position.y = self.following_local_pose[id].pose.position.y + self.bais[id][1]
        self.following_local_pose[id].pose.position.z = self.following_local_pose[id].pose.position.z
        if not id in self.fly_success_num:
            self.fly_success_num.append(id)

    def auction_callback(self, msg, id): #
        self.task_num = id
        self.auction_count = 0
        # if id == self.id:
        #     self.waitingforauction_flag = True   # stop working and wait for the result
        # if self.reset_auction_flag:
        #     self.task_count = 0
        #     self.reset_auction_flag = False
        if self.auction_finish_flag: 
            self.auction_finish_flag = False   # auction start!
            self.task_flag = False  # join in the auction and no one have won the bidding.
            self.task_position = msg
            #print msg
            if (self.situation_flag == 1) or (self.situation_flag == 2) or (self.situation_flag == 4) or (self.situation_flag == 5): # bid offer a price.
                distance = ((self.uav_current_pose.x - self.task_position.actor_x) ** 2 + (self.uav_current_pose.y - self.task_position.actor_y) ** 2)**0.5
                if distance < 20.0:
                    self.auction_value.value = 1 / (distance + 1)  # bid offer a price.
                else:
                    self.auction_value.value = 0.0
            # elif self.situation_flag == 3:
            #     delta_time = rospy.get_time()-self.track_time
            #     if delta_time > 15:
            #         distance = (self.uav_current_pose.x - self.task_position.actor_x) ** 2 + (self.uav_current_pose.y - self.task_position.actor_y) ** 2
            #         time_lose = (20 - delta_time)/20
            #         self.auction_value.value = 1 / (distance + 1) - time_lose
            #     else:
            #         self.auction_value.value = 0.0
            else:
                self.auction_value.value = 0.0
            self.auction_value.value = self.auction_value.value + id * 10.0
            print 'auction_value' + str(self.id) +':', self.auction_value.value
            self.file.write('auction_value' + str(self.id) +':'+str(self.auction_value.value))
            self.file.write('\n')
            self.auction_value_pub.publish(self.auction_value)

    def auction_value_callback(self, msg): 
        self.task_count += 1
        other_value = msg
        if other_value.value >= self.task_num*10.0:
            self.auction_value_all.append(other_value.value)
            fly_succ_num = len(self.fly_success_num)+1     
            if len(self.auction_value_all) == fly_succ_num:
                #print 'max(self.auction_value_all):', max(self.auction_value_all)
                max_value = max(self.auction_value_all)
                if max_value > self.task_num*10.0:
                    self.free_uav_flag = True
                    if abs(max_value - self.auction_value.value) < 0.00001:  # i win the auction
                        self.situation_flag = 5
                        self.arrive_point = False
                        self.change_task_flag = True
                        print 'auction_winer:' + str(self.id)
                        self.file.write('auction_winer:' + str(self.id))
                        self.file.write('\n')
                self.task_count = 0
                self.auction_finish_flag = True
                # self.waitingforauction_flag = False
                self.auction_value_all = []   # this auction is over.
                self.auction_count = 0
                
        else:  # there is something wrong with the auction, ask for resuming.
            self.auction_finish_flag = True
            # self.reset_auction_flag = True
            self.task_flag = False
            self.task_count = 0

    def tracking_callback(self, msg):
        track_value = msg.value
        if track_value < 150.00:
            self.situation_flag = 3
            #self.situation2_flag.situation2_id = 0 * 100 + self.id
            #self.situation2_id_pub.publish(self.situation2_flag)
            self.track_time = rospy.get_time()
            #print str(self.id) +'i got it '
            self.file.write(str(self.id) +'i got it ')
            self.file.write('\n')
        elif track_value > 150.00:
            self.change_task_flag = True
            self.arrive_point = False
            self.situation_flag = 4
            #print str(self.id) +'i am coming back'
            self.file.write(str(self.id) +'i am coming back')
            self.file.write('\n')
    def loop(self):
        rate = rospy.Rate(self.f)
        count_situ_one = 0
        for i in range(self.uav_num):
            if not i == self.id:
                self.following_local_pose_sub[i] = rospy.Subscriber(self.uav_type + '_' + str(i) + "/mavros/local_position/pose", PoseStamped, self.following_local_pose_callback, i)
            self.auction_sub[i] = rospy.Subscriber(self.uav_type+'_'+str(i)+'/auction',auction,self.auction_callback,i)
        while not rospy.is_shutdown():
            self.count += 1
            self.velxy_max = 4.0
            self.cmd = ''
            # fly to the same altitude  xian offboard hou arm
            if self.count == 40 or self.count == 42 or self.count == 44:
                self.cmd = 'OFFBOARD'
            if self.count == 94 or self.count == 96 or self.count == 98:
                self.cmd = 'ARM'
            if self.situation_flag == 0:    # ding gao
                if self.count < 2:
                    self.takeoff_position.position.z = self.z
                    self.takeoff_position.position.x = self.uav_current_pose.x
                    self.takeoff_position.position.y = self.uav_current_pose.y
                    self.pose_cmd = self.construct_target()
                    self.takeoff_pub.publish(self.pose_cmd)
                    if not self.arm_state:
                        self.arm()
                    if self.mavros_state != 'OFFBOARD':
                        self.flight_mode_set(mode='OFFBOARD')
            # fly to the initial point
            if self.situation_flag == 1 and self.change_task_flag:  # chu shi hua
                self.change_task_flag = False
                
                #if not self.avoid_start_flag:
                self.init_point()
                # self.Obstable_uav_object()

            # fly in the way
            if self.situation_flag == 2 and (self.change_task_flag or self.arrive_point):  # sou suo
                print str(self.id) + ':   flag == 2 ~~~~~~'
                self.file.write(str(self.id) +':   flag == 2 ~~~~~~')
                self.file.write('\n')
                if self.circle0_flag:
                    self.file.write('circle0       flag :  2' )
                    self.file.write('\n')
                    self.circle0_plan()
                elif self.circle1_flag:
                    self.file.write('circle1       flag :  2' )
                    self.file.write('\n')
                    self.circle1_plan()
                elif self.circle2_flag:
                    self.file.write('circle2       flag :  2' )
                    self.file.write('\n')
                    self.circle2_plan()
                elif self.circle3_flag:
                    self.file.write('circle3       flag :  2' )
                    self.file.write('\n')
                    self.circle3_plan()
                elif self.circle4_flag:
                    self.file.write('circle4       flag :  2' )
                    self.file.write('\n')
                    self.circle4_plan()
                elif self.circle5_flag:
                    self.file.write('circle5       flag :  2' )
                    self.file.write('\n')
                    self.circle5_plan()
                    # control for uniform distribution
                    #self.control_uniform()
                #else:
                    #self.circle5_flag = True
                self.change_task_flag = False
                self.avoid_finish_flag = False
                #self.init_point()
                # self.Obstable_uav_object()
                    # control for uniform distribution
                    # self.control_uniform()
                # else:
                #     self.outer_flag = True
                # self.change_task_flag = False

            if self.situation_flag == 3:  # tracking
                # catch actors: tacking_callback
                # if self.count % 500 == 0:
                pass
                    # print str(self.id) + ':i got it '

            if self.situation_flag == 4:  # tracking over
                if self.change_task_flag:
                    print str(self.id) + ':   flag == 4 ~~~~~~'
                    self.file.write(str(self.id) + ':   flag == 4 ~~~~~~')
                    self.file.write('\n')
                    self.subtarget_count = 0
                    self.uav_vel.linear.x = 0.0
                    self.uav_vel.linear.y = 0.0
                    self.uav_vel.linear.z = 0.0
                    self.uav_vel.angular.z = 0.0
                    self.takeoff_pub.publish(self.vel_cmd())
                    self.change_task_flag = False
                    self.change_task()
                    self.Obstable_uav_object()

            if self.situation_flag == 5: # win the auction and go to the position
                if self.change_task_flag:
                    self.change_task_flag = False
                    self.subtarget_count = 0
                    self.target_position.linear.x = self.task_position.actor_x
                    self.target_position.linear.y = self.task_position.actor_y
                    dis_list = [0,0,0,0]
                    # check if targetposition is in the blackbox:
                    for i in range(self.box_num): # ru guo di ren wei zhi zai zhang ai wu nei, ze qian wang zhang ai wu wai zui jin de wei zhi
                        if (self.target_position.linear.x > self.black_box[i][0][0]-1.0) and (self.target_position.linear.x < self.black_box[i][0][1]+1.0):
                            if (self.target_position.linear.y > self.black_box[i][1][0]-1.0) and (self.target_position.linear.y < self.black_box[i][1][1]+1.0):
                                dis_list[0] = abs(self.target_position.linear.x - (self.black_box[i][0][0]-1.0))
                                dis_list[1] = abs(self.target_position.linear.x - (self.black_box[i][0][1]+1.0))
                                dis_list[2] = abs(self.target_position.linear.y - (self.black_box[i][1][0]-1.0))
                                dis_list[3] = abs(self.target_position.linear.y - (self.black_box[i][1][1]+1.0))
                                dis_min = dis_list.index(min(dis_list))
                                if dis_min ==0:
                                    self.target_position.linear.x = self.black_box[i][0][0]-1.0
                                elif dis_min ==1:
                                    self.target_position.linear.x = self.black_box[i][0][1]+1.0
                                elif dis_min ==2:
                                    self.target_position.linear.y = self.black_box[i][1][0]-1.0
                                else:
                                    self.target_position.linear.y = self.black_box[i][1][1]+1.0
                                break
                    self.Obstable_uav_object()

            # judge if arrive the target point
            distance_tar_cur = self.VectNorm3(self.target_position.linear, self.uav_current_pose)
            if  distance_tar_cur < 1:
                self.arrive_count += 1
                #print("arrive_count:",self.arrive_count)
                if self.arrive_count > 2:
                    self.arrive_point = True
                    self.arrive_count = 0
                else:
                    self.arrive_point = False
            else:
                self.arrive_count = 0
                self.arrive_point = False
            
            if self.situation_flag == 1:
                count_situ_one += 1
                if count_situ_one == 5*self.f:
                    self.start_yolo_pub.publish('gogogo')

            # task changes:
            if (self.situation_flag == 0) and self.arrive_point:
                self.change_task_flag = True
                self.situation_flag = 1
                self.arrive_point = False
                self.file.write('flag:0 to 1')
                self.file.write('\n')
            elif (self.situation_flag == 1) and self.arrive_point and self.avoid_finish_flag:
                #self.avoid_start_flag = False
                self.situation_flag = 2
                # self.start_yolo_pub.publish('gogogo')
                self.arrive_point = False
                self.change_task_flag = True
                self.file.write('flag:1 to 2')
                self.file.write('\n')

            elif (self.situation_flag == 4) and self.arrive_point and self.avoid_finish_flag:
                self.situation_flag = 2
                self.change_task_flag = True
                self.arrive_point = False
                self.file.write('flag:4 to 2')
                self.file.write('\n')
                
            elif (self.situation_flag == 5) and self.arrive_point and self.avoid_finish_flag:
                self.arrive_point = False
                print str(self.id) +'i am arriving'
                self.file.write('flag:5 to 6')
                self.file.write('\n')
                self.situation_flag = 6
                self.search_flag = False
                self.last_yaw = self.uav_current_yaw
                    
            if self.situation_flag == 6:
                if not self.search_flag:
                    self.uav_vel.linear.x = 0.0
                    self.uav_vel.linear.y = 0.0
                    self.uav_vel.linear.z = 0.0
                    self.uav_vel.angular.z = 0.5
                    if abs(self.uav_current_yaw - self.last_yaw) < 0.2:
                        self.search_flag = True
                        self.situation_flag = 4
                        self.change_task_flag = True
                        self.file.write('flag:6 to 4')
                        self.file.write('\n')
                        
            # avoidance and control
            if (self.situation_flag == 4 or self.situation_flag == 5 or self.situation_flag == 1) and (not self.avoid_finish_flag):
                if self.arrive_point:
                    self.subtarget_count += 1
                    self.arrive_point = False
                    if self.subtarget_count == self.subtarget_length:
                        if self.mid_avoid_flag:
                            self.target_position.linear = self.mid_target_position
                            self.Obstable_uav_object()
                        else:
                            self.avoid_finish_flag = True
                        self.subtarget_count = 0
                    else:
                        self.target_position.linear.x = self.middd_pos[self.subtarget_count].x
                        self.target_position.linear.y = self.middd_pos[self.subtarget_count].y
                        # print('target_position:'+str(self.id), self.target_position)
                        # print('current_position:'+str(self.id), self.uav_current_pose)
            
            if self.situation_flag == 1:
                self.velxy_max = 5
            
            if self.situation_flag != 3:
                self.get_control_vel()
                self.obstacle_avoid()
                self.vel_enu_pub.publish(self.uav_vel)
                self.cmd_pub.publish(self.cmd)

            rate.sleep()
        self.file.close()
    # ji wu bi zhang
    def Obstable_uav_object(self):
        self.mid_avoid_flag = False
        print str(self.id) + 'self.uav_curr_pose:   ', self.uav_current_pose
        self.file.write(str(self.id) + 'self.uav_curr_pose:   '+str(self.uav_current_pose))
        self.file.write('\n')
        print str(self.id) + 'self.target_pose:     ', self.target_position.linear
        self.file.write(str(self.id) + 'self.target_pose:     '+str(self.target_position.linear))
        self.file.write('\n')
        self.mid_target_position = self.target_position.linear
        try:   # fang zhi chu xian wu jie de qing kuang
            subtarget_pos = self.Obstacleavoid.GetPointList(self.uav_current_pose, self.target_position.linear, 1) # current pose, target pose, safe distance
            self.subtarget_length = len(subtarget_pos)
            self.middd_pos = [Point() for k in range(self.subtarget_length)]
            self.middd_pos = copy.deepcopy(subtarget_pos)
            self.target_position.linear.x = copy.deepcopy(self.middd_pos[0].x)
            self.target_position.linear.y = copy.deepcopy(self.middd_pos[0].y)
            #print str(self.id) + 'middd_pos:',self.middd_pos
            self.file.write(str(self.id) + 'middd_pos:'+str(self.middd_pos))
            self.file.write('\n')
        except: # ru guo wu jie shuo ming dang qian uav de wei zhi zai zhang ai wu fan wei nei,
            for i in range(self.box_num):
                if (self.target_position.linear.x > self.black_box[i][0][0]-1.0) and (self.target_position.linear.x < self.black_box[i][0][1]+1.0):
                    if (self.target_position.linear.y > self.black_box[i][1][0]-1.0) and (self.target_position.linear.y < self.black_box[i][1][1]+1.0):
                        dis_list = [0,0,0,0]
                        dis_list[0] = abs(self.uav_current_pose.x - (self.black_box[i][0][0]-1.0))
                        dis_list[1] = abs(self.uav_current_pose.x - (self.black_box[i][0][1]+1.0))
                        dis_list[2] = abs(self.uav_current_pose.y - (self.black_box[i][1][0]-1.0))
                        dis_list[3] = abs(self.uav_current_pose.y - (self.black_box[i][1][1]+1.0))
                        dis_min = dis_list.index(min(dis_list))
                        if dis_min ==0:
                            self.target_position.linear.x = self.black_box[i][0][0]-1.0
                            self.target_position.linear.y = self.uav_current_pose.y
                        elif dis_min ==1:
                            self.target_position.linear.x = self.black_box[i][0][1]+1.0
                            self.target_position.linear.y = self.uav_current_pose.y
                        elif dis_min ==2:
                            self.target_position.linear.x = self.uav_current_pose.x
                            self.target_position.linear.y = self.black_box[i][1][0]-1.0
                        else:
                            self.target_position.linear.x = self.uav_current_pose.x
                            self.target_position.linear.y = self.black_box[i][1][1]+1.0                                       
                        self.subtarget_length = 1  
                        self.mid_avoid_flag = True 
                        break
            print str(self.id) + 'except:', self.target_position.linear
            self.file.write(str(self.id) + 'middd_pos:'+str(self.target_position.linear))
            self.file.write('\n')
        self.avoid_finish_flag = False
    # target point to cmd vel
    def get_control_vel(self): # P kong zhi, flu zuo biao xi
        
        uav_dis_curtar = self.VectNorm2(self.target_position.linear, self.uav_current_pose)   #distance
        temp = self.VectDiff(self.target_position.linear, self.uav_current_pose)  # vector
        uav_vel_total = self.Kp * uav_dis_curtar  # velocity
        if uav_vel_total > self.velxy_max:
            uav_vel_total = self.velxy_max
        '''
        if not uav_dis_curtar == 0.0:
            self.uav_vel.linear.x = (temp.x/uav_dis_curtar) * uav_vel_total
            self.uav_vel.linear.y = (temp.y/uav_dis_curtar) * uav_vel_total
        else:
            self.uav_vel.linear.x = 0.0
            self.uav_vel.linear.y = 0.0

        if self.uav_vel.linear.z > self.velz_max:
            self.uav_vel.linear.z = self.velz_max
        elif self.uav_vel.linear.z < - self.velz_max:
            self.uav_vel.linear.z = - self.velz_max
        '''
        
        turnover_flag = False
        self.target_yaw = self.pos2ang(self.target_position.linear.x, self.target_position.linear.y, self.uav_current_pose.x, self.uav_current_pose.y)
        mid_yaw = self.target_yaw - self.uav_current_yaw
        if mid_yaw > math.pi:
            mid_yaw = mid_yaw - 2*math.pi
        elif mid_yaw < -math.pi:
            mid_yaw = 2*math.pi + mid_yaw
        self.uav_vel.angular.z = self.Kpy * mid_yaw
        if self.uav_vel.angular.z > self.angz_max:
            self.uav_vel.angular.z = self.angz_max
        elif self.uav_vel.angular.z < -self.angz_max:
            self.uav_vel.angular.z = -self.angz_max
        self.uav_vel.linear.x  = uav_vel_total * math.cos(mid_yaw)
        self.uav_vel.linear.y = uav_vel_total * math.sin(mid_yaw)

        self.uav_vel.linear.z = self.Kp * (self.target_position.linear.z - self.uav_current_pose.z)
                       
    def init_point(self):
        if self.id == 0:  # middle circle  0
            self.target_position.linear.x = 10.0
            self.target_position.linear.y = 0.0
            self.target_position.linear.z = self.z
            self.circle0_flag = True
        elif self.id == 5:  # middle circle  5
            self.target_position.linear.x =3.0
            self.target_position.linear.y = 15.0
            self.target_position.linear.z = self.z
            self.circle5_flag = True
        elif self.id == 2:  # middle circle  2
            self.target_position.linear.x =-10.0
            self.target_position.linear.y = 0.0
            self.target_position.linear.z = self.z
            self.circle2_flag = True
        elif self.id == 1:  # middle circle  1
            self.target_position.linear.x = 3.0
            self.target_position.linear.y = 0.0
            self.target_position.linear.z = self.z
            self.circle1_flag = True
        elif self.id == 4:  # middle circle  4
            self.target_position.linear.x = -10.0
            self.target_position.linear.y = 9.0
            self.target_position.linear.z = self.z
            self.circle4_flag = True
        elif self.id == 3:  # middle circle  3
            self.target_position.linear.x = 5.0
            self.target_position.linear.y = 6.0
            self.target_position.linear.z = self.z
            self.circle3_flag = True

    def circle0_plan(self):
        # self.circle2_flag = False
        self.circle1_flag = False
        self.circle2_flag = False
        self.circle3_flag = False
        self.circle4_flag = False
        self.circle5_flag = False
        self.velxy_max = 3
        size0 = len(self.circle0_pose)
        if self.change_task_flag:  # di yi ci jin ru sou suo mo shi xu yao pan duan xia yi ge mu biao dian de suo yin zhi
            self.next_node = 0
            #self.situation2_flag.situation2_id = 1 * 100 + self.id
            n = 0
            for i in self.circle0_pose:
                n = n+1
                if abs(self.uav_current_pose.x - i[0]) < 1:
                    self.next_node = n
                    break
                elif abs(self.uav_current_pose.y - i[1]) < 1:
                    if n == 1:
                        self.next_node = 0
                    else:
                        self.next_node = n
                    break
            self.target_position.linear.x = self.circle0_pose[self.next_node][0]
            self.target_position.linear.y = self.circle0_pose[self.next_node][1]
            self.change_task_flag = False
            #self.situation2_id_pub.publish(self.situation2_flag)  
        elif self.arrive_point:
            self.target_position.linear.x = self.circle0_pose[self.next_node][0]
            self.target_position.linear.y = self.circle0_pose[self.next_node][1]
            self.next_node += 1
            if self.next_node == size0:
                self.next_node = 0 
    
    def circle1_plan(self):
        # self.circle2_flag = False
        self.circle0_flag = False
        self.circle2_flag = False
        self.circle3_flag = False
        self.circle4_flag = False
        self.circle5_flag = False
        self.velxy_max = 3
        size1 = len(self.circle1_pose)
        if self.change_task_flag:  # di yi ci jin ru sou suo mo shi xu yao pan duan xia yi ge mu biao dian de suo yin zhi
            self.next_node = 0
            #self.situation2_flag.situation2_id = 1 * 100 + self.id
            n = 0
            for i in self.circle1_pose:
                n = n+1
                if abs(self.uav_current_pose.x - i[0]) < 1:
                    self.next_node = n
                    break
                elif abs(self.uav_current_pose.y - i[1]) < 1:
                    if n == 1:
                        self.next_node = 0
                    else:
                        self.next_node = n
                    break
            self.target_position.linear.x = self.circle1_pose[self.next_node][0]
            self.target_position.linear.y = self.circle1_pose[self.next_node][1]
            self.change_task_flag = False
            #self.situation2_id_pub.publish(self.situation2_flag)  
        elif self.arrive_point:
            self.target_position.linear.x = self.circle1_pose[self.next_node][0]
            self.target_position.linear.y = self.circle1_pose[self.next_node][1]
            self.next_node += 1
            if self.next_node == size1:
                self.next_node = 0 

    def circle2_plan(self):
        # self.circle2_flag = False
        self.circle0_flag = False
        self.circle1_flag = False
        self.circle3_flag = False
        self.circle4_flag = False
        self.circle5_flag = False
        self.velxy_max = 3
        size2 = len(self.circle2_pose)
        if self.change_task_flag:  # di yi ci jin ru sou suo mo shi xu yao pan duan xia yi ge mu biao dian de suo yin zhi
            self.next_node = 0
            #self.situation2_flag.situation2_id = 1 * 100 + self.id
            n = 0
            for i in self.circle2_pose:
                n = n+1
                if abs(self.uav_current_pose.x - i[0]) < 1:
                    self.next_node = n
                    break
                elif abs(self.uav_current_pose.y - i[1]) < 1:
                    if n == 1:
                        self.next_node = 0
                    else:
                        self.next_node = n
                    break
            self.target_position.linear.x = self.circle2_pose[self.next_node][0]
            self.target_position.linear.y = self.circle2_pose[self.next_node][1]
            self.change_task_flag = False
            #self.situation2_id_pub.publish(self.situation2_flag)  
        elif self.arrive_point:
            self.target_position.linear.x = self.circle2_pose[self.next_node][0]
            self.target_position.linear.y = self.circle2_pose[self.next_node][1]
            self.next_node += 1
            if self.next_node == size2:
                self.next_node = 0 

    def circle3_plan(self):
        # self.circle2_flag = False
        self.circle0_flag = False
        self.circle1_flag = False
        self.circle2_flag = False
        self.circle4_flag = False
        self.circle5_flag = False
        self.velxy_max = 3
        size3 = len(self.circle3_pose)
        if self.change_task_flag:  # di yi ci jin ru sou suo mo shi xu yao pan duan xia yi ge mu biao dian de suo yin zhi
            self.next_node = 0
            #self.situation2_flag.situation2_id = 1 * 100 + self.id
            n = 0
            for i in self.circle3_pose:
                n = n+1
                if abs(self.uav_current_pose.x - i[0]) < 1:
                    self.next_node = n
                    break
                elif abs(self.uav_current_pose.y - i[1]) < 1:
                    if n == 1:
                        self.next_node = 0
                    else:
                        self.next_node = n
                    break
            self.target_position.linear.x = self.circle3_pose[self.next_node][0]
            self.target_position.linear.y = self.circle3_pose[self.next_node][1]
            self.change_task_flag = False
            #self.situation2_id_pub.publish(self.situation2_flag)  
        elif self.arrive_point:
            self.target_position.linear.x = self.circle3_pose[self.next_node][0]
            self.target_position.linear.y = self.circle3_pose[self.next_node][1]
            self.next_node += 1
            if self.next_node == size3:
                self.next_node = 0 

    def circle4_plan(self):
        # self.circle2_flag = False
        self.circle0_flag = False
        self.circle2_flag = False
        self.circle3_flag = False
        self.circle1_flag = False
        self.circle5_flag = False
        self.velxy_max = 3
        size4 = len(self.circle4_pose)
        if self.change_task_flag:  # di yi ci jin ru sou suo mo shi xu yao pan duan xia yi ge mu biao dian de suo yin zhi
            self.next_node = 0
            #self.situation2_flag.situation2_id = 1 * 100 + self.id
            n = 0
            for i in self.circle4_pose:
                n = n+1
                if abs(self.uav_current_pose.x - i[0]) < 1:
                    self.next_node = n
                    break
                elif abs(self.uav_current_pose.y - i[1]) < 1:
                    if n == 1:
                        self.next_node = 0
                    else:
                        self.next_node = n
                    break
            self.target_position.linear.x = self.circle4_pose[self.next_node][0]
            self.target_position.linear.y = self.circle4_pose[self.next_node][1]
            self.change_task_flag = False
            #self.situation2_id_pub.publish(self.situation2_flag)  
        elif self.arrive_point:
            self.target_position.linear.x = self.circle4_pose[self.next_node][0]
            self.target_position.linear.y = self.circle4_pose[self.next_node][1]
            self.next_node += 1
            if self.next_node == size4:
                self.next_node = 0 

    def circle5_plan(self):
        # self.circle2_flag = False
        self.circle0_flag = False
        self.circle2_flag = False
        self.circle3_flag = False
        self.circle4_flag = False
        self.circle1_flag = False
        self.velxy_max = 3
        size5 = len(self.circle5_pose)
        if self.change_task_flag:  # di yi ci jin ru sou suo mo shi xu yao pan duan xia yi ge mu biao dian de suo yin zhi
            self.next_node = 0
            #self.situation2_flag.situation2_id = 1 * 100 + self.id
            n = 0
            for i in self.circle5_pose:
                n = n+1
                if abs(self.uav_current_pose.x - i[0]) < 1:
                    self.next_node = n
                    break
                elif abs(self.uav_current_pose.y - i[1]) < 1:
                    if n == 1:
                        self.next_node = 0
                    else:
                        self.next_node = n
                    break
            self.target_position.linear.x = self.circle5_pose[self.next_node][0]
            self.target_position.linear.y = self.circle5_pose[self.next_node][1]
            self.change_task_flag = False
            #self.situation2_id_pub.publish(self.situation2_flag)  
        elif self.arrive_point:
            self.target_position.linear.x = self.circle5_pose[self.next_node][0]
            self.target_position.linear.y = self.circle5_pose[self.next_node][1]
            self.next_node += 1
            if self.next_node == size5:
                self.next_node = 0 


    # def circle2_plan(self):
    #     self.circle1_flag = False
    #     self.outer_flag = False
    #     self.velxy_max = 3
    #     size2 = len(self.circle2_pose)
    #     if self.change_task_flag:
    #         self.next_node = 0
    #         self.situation2_flag.situation2_id = 1 * 100 + self.id
    #         n = 0
    #         for i in self.circle2_pose:
    #             n = n+1
    #             if abs(self.uav_current_pose.x - i[0]) < 1:
    #                 self.next_node = n
    #                 break
    #             elif abs(self.uav_current_pose.y - i[1]) < 1:
    #                 if n == 1:
    #                     self.next_node = 0
    #                 else:
    #                     self.next_node = n
    #                 break
    #         self.target_position.linear.x = self.circle2_pose[self.next_node][0]
    #         self.target_position.linear.y = self.circle2_pose[self.next_node][1]
    #         self.change_task_flag = False
    #         self.situation2_id_pub.publish(self.situation2_flag)   
    #     elif self.arrive_point:
    #         self.target_position.linear.x = self.circle2_pose[self.next_node][0]
    #         self.target_position.linear.y = self.circle2_pose[self.next_node][1]
    #         self.next_node += 1
    #         if self.next_node == size2:
    #             self.next_node = 0 

    # def outer_plan(self):
    #     self.circle1_flag = False
    #     # self.circle2_flag = False
    #     self.velxy_max = 3
    #     size3 = len(self.outer_pose)
    #     if self.change_task_flag:
    #         self.next_node = 0
    #         self.situation2_flag.situation2_id = 1 * 100 + self.id
    #         n = 0
    #         for i in self.outer_pose:
    #             n = n+1
    #             if abs(self.uav_current_pose.x - i[0]) < 1:
    #                 self.next_node = n
    #                 break
    #             elif abs(self.uav_current_pose.y - i[1]) < 1:
    #                 if n == 1:
    #                     self.next_node = 0
    #                 else:
    #                     self.next_node = n
    #                 break
    #         self.target_position.linear.x = self.outer_pose[self.next_node][0]
    #         self.target_position.linear.y = self.outer_pose[self.next_node][1]
    #         self.change_task_flag = False
    #         self.situation2_id_pub.publish(self.situation2_flag)   
    #     elif self.arrive_point:
    #         self.target_position.linear.x = self.outer_pose[self.next_node][0]
    #         self.target_position.linear.y = self.outer_pose[self.next_node][1]
    #         self.next_node += 1
    #         if self.next_node == size3:
    #             self.next_node = 0

    # keep distance between two uavs on outer trajectory
    def control_uniform(self):
        angle = [0 for j in range(self.uav_num)]
        situation2_num = 0
        for i in self.situation2_list:
            if i == 1:
                situation2_num += 1
        if situation2_num > 1:
            exp = (2 * math.pi) / situation2_num
            mid_ang = 0.0
            for i in range(self.uav_num):
                if i == self.id:
                    angle[self.id] = self.pos2ang(self.uav_current_pose.x, self.uav_current_pose.y, 50.0, 0.0)
                else:
                    if self.situation2_list[i] == 1:
                        angle[i] = self.pos2ang(self.following_local_pose[i].pose.position.x, self.following_local_pose[i].pose.position.y, 50.0, 0.0)
                    else:
                        angle[i] = 100
            sort_uavs = numpy.argsort(angle)  
            sort_uavs = sort_uavs[0:situation2_num]
            # 4 xiang xian
            four_quadrant = [0, 1, 2, 3]
            dot_quadrant = [[[130.0, 0.0],[50.0,-40.0]],[[50.0,40.0],[130.0,0.0]],[[-40.0,0.0],[50.0,40.0]],[[-40.0,0.0],[50.0,-40.0]]]
            for i in range(situation2_num):
                if (angle[sort_uavs[i]]>0) and (angle[sort_uavs[i]]<= math.pi/2):
                    if 0 in four_quadrant:
                        four_quadrant.remove(0)
                elif (angle[sort_uavs[i]]>math.pi/2) and (angle[sort_uavs[i]]<= math.pi):
                    if 1 in four_quadrant:
                        four_quadrant.remove(1)
                elif (angle[sort_uavs[i]]>math.pi) and (angle[sort_uavs[i]]<= 3*math.pi/2):
                    if 2 in four_quadrant:
                        four_quadrant.remove(2)
                elif (angle[sort_uavs[i]]>3*math.pi/2) and (angle[sort_uavs[i]]<= math.pi):
                    if 3 in four_quadrant:
                        four_quadrant.remove(3)
            dis2outer = [0.0 for k in range(4)]
            dis2outer1 = [[0.0, 0.0] for k in range(4)]
            for j in four_quadrant:
                dis2outer1[j][0] = (self.uav_current_pose.x-dot_quadrant[j][0][0])**2+ (self.uav_current_pose.y-dot_quadrant[j][0][1])**2
                dis2outer1[j][1] = (self.uav_current_pose.x-dot_quadrant[j][1][0])**2+ (self.uav_current_pose.y-dot_quadrant[j][1][1])**2
                dis2outer[j] = min(dis2outer1[j])
            min_dis = min(dis2outer)
            dis_index = dis2outer.index(min_dis)   # j
            index2outer = dis2outer1[dis_index].index(min_dis)  # 1 or 2
            self.target_position.linear.x = dot_quadrant[dis_index][index2outer][0]
            self.target_position.linear.y = dot_quadrant[dis_index][index2outer][1]

    def change_task(self):
        print("change task to cruise")
        if self.circle0_flag:
            self.file.write("circle0_flag")
            self.file.write('\n')
            if self.uav_current_pose.x > self.circle0_center.actor_x:
                if self.uav_current_pose.y > self.circle0_center.actor_y:
                    self.target_position.linear.x = 45.0
                    self.target_position.linear.y = -5.0
                else: 
                    self.target_position.linear.x = 45.0
                    self.target_position.linear.y = -40.0
            else:
                if self.uav_current_pose.y > self.circle0_center.actor_y:
                    self.target_position.linear.x = -40.0
                    self.target_position.linear.y = 0.0
                else:
                    self.target_position.linear.x = -40.0
                    self.target_position.linear.y = -38.0
        elif self.circle1_flag:
            self.file.write("circle1_flag")
            self.file.write('\n')
            if self.uav_current_pose.x > self.circle1_center.actor_x:
                if self.uav_current_pose.y > self.circle1_center.actor_y:
                    self.target_position.linear.x = 70.0
                    self.target_position.linear.y = 0.0
                else: 
                    self.target_position.linear.x = 110.0
                    self.target_position.linear.y = -40.0
            else:
                if self.uav_current_pose.y > self.circle1_center.actor_y:
                    self.target_position.linear.x = 17.0
                    self.target_position.linear.y = 0.0
                else:
                    self.target_position.linear.x = 17.0
                    self.target_position.linear.y = -45.0
        elif self.circle2_flag:
            self.file.write("circle2_flag")
            self.file.write('\n')
            if self.uav_current_pose.x > self.circle2_center.actor_x:
                if self.uav_current_pose.y > self.circle2_center.actor_y:
                    self.target_position.linear.x = -10.0
                    self.target_position.linear.y = 45.0
                else: 
                    self.target_position.linear.x = -15.0
                    self.target_position.linear.y = -40.0
            else:
                if self.uav_current_pose.y > self.circle2_center.actor_y:
                    self.target_position.linear.x = -40.0
                    self.target_position.linear.y = 45.0
                else:
                    self.target_position.linear.x = -40.0
                    self.target_position.linear.y = -40.0
        elif self.circle3_flag:
            self.file.write("circle3_flag")
            self.file.write('\n')
            if self.uav_current_pose.x > self.circle3_center.actor_x:
                if self.uav_current_pose.y > self.circle3_center.actor_y:
                    self.target_position.linear.x = 110.0
                    self.target_position.linear.y = 40.0
                else: 
                    self.target_position.linear.x = 110.0
                    self.target_position.linear.y = -45.0
            else:
                if self.uav_current_pose.y > self.circle3_center.actor_y:
                    self.target_position.linear.x = 50.0
                    self.target_position.linear.y = 40.0
                else:
                    self.target_position.linear.x = 50.0
                    self.target_position.linear.y = -45.0
        elif self.circle4_flag:
            self.file.write("circle4_flag")
            self.file.write('\n')
            if self.uav_current_pose.x > self.circle4_center.actor_x:
                if self.uav_current_pose.y > self.circle4_center.actor_y:
                    self.target_position.linear.x = 40.0
                    self.target_position.linear.y = 40.0
                else: 
                    self.target_position.linear.x = 40.0
                    self.target_position.linear.y = 0.0
            else:
                if self.uav_current_pose.y > self.circle4_center.actor_y:
                    self.target_position.linear.x = -45.0
                    self.target_position.linear.y = 40.0
                else:
                    self.target_position.linear.x = -45.0
                    self.target_position.linear.y = 10.0
        elif self.circle5_flag:
            self.file.write("circle5_flag")
            self.file.write('\n')
            if self.uav_current_pose.x > self.circle5_center.actor_x:
                if self.uav_current_pose.y > self.circle5_center.actor_y:
                    self.target_position.linear.x = 120.0
                    self.target_position.linear.y = 40.0
                else: 
                    self.target_position.linear.x = 120.0
                    self.target_position.linear.y = 0.0
            else:
                if self.uav_current_pose.y > self.circle5_center.actor_y:
                    self.target_position.linear.x = 0.0
                    self.target_position.linear.y = 40.0
                else:
                    self.target_position.linear.x = 0.0
                    self.target_position.linear.y = 0.0
        # else:
        #     self.control_uniform()

    # ji jian bi zhang
    def obstacle_avoid(self):
        self.avo_id = []
        for i in range(self.uav_num):
            if not i == self.id:
                dis_partner = math.sqrt(
                    (self.uav_current_pose.x - self.following_local_pose[i].pose.position.x) ** 2 + (self.uav_current_pose.y - self.following_local_pose[i].pose.position.y) ** 2)
                if dis_partner < self.safe_dis:
                    if (self.uav_current_pose.z - self.following_local_pose[i].pose.position.z) < self.safe_height:
                        self.avo_id.append(i)
        avoid_num = len(self.avo_id)
        heigher_num = 0
        if avoid_num > 0:
            for j in range(avoid_num):
                if self.following_local_pose[self.avo_id[j]].pose.position.z > self.uav_current_pose.z:
                    heigher_num = heigher_num + 1
            if heigher_num == 0:
                self.target_position.linear.z = self.z + self.safe_height
            else:
                self.target_position.linear.z = self.z - self.safe_height * heigher_num
        else:
            self.target_position.linear.z = self.z

    def pos2ang(self, xa, ya, xb, yb):   #([xb,yb] to [xa, ya])
        if not xa-xb == 0:
            angle = math.atan2((ya - yb),(xa - xb))
            if (ya-yb > 0) and (angle < 0):
                angle = angle + math.pi
            elif (ya-yb < 0) and (angle > 0):
                angle = angle - math.pi
            elif ya-yb == 0:
                if xa-xb > 0:
                    angle = 0.0
                else:
                    angle = math.pi
        else:
            if ya-yb > 0:
                angle = math.pi / 2
            elif ya-yb <0:
                angle = -math.pi / 2
            else:
                angle = 0.0
        if angle < 0:
            angle = angle + 2 * math.pi   # 0 to 2pi
        return angle

    def VectNorm3(self, Pt1, Pt2):
        norm = math.sqrt(pow(Pt1.x - Pt2.x, 2) + pow(Pt1.y - Pt2.y, 2)+ pow(Pt1.z - Pt2.z, 2))
        return norm

    def VectNorm2(self, Pt1, Pt2):
        norm = math.sqrt(pow(Pt1.x - Pt2.x, 2) + pow(Pt1.y - Pt2.y, 2))
        return norm

    def VectDiff(self, endPt, startPt):
        temp = Point()
        temp.x = endPt.x - startPt.x
        temp.y = endPt.y - startPt.y
        return temp
if __name__ == '__main__':
    catchplan = CatchingPlan(sys.argv[1])
    catchplan.loop()
